//
// Created by jg on 2021/1/5.
//

#ifndef WMR_PLAN_MANAGE_PLAN_CONTAINER_HPP
#define WMR_PLAN_MANAGE_PLAN_CONTAINER_HPP

#include <eigen3/Eigen/Dense>
#include <vector>
#include <ros/ros.h>

#include <fast_marching/fast_marching.h>
#include <bezier/bezier_base.h>
#include <bezier_opt/trajectory_generator.h>
#include <topo_prm/topo_prm.h>

using std::vector;
using std::list;

struct PlanParameters {
    /* planning algorithm parameters */
    double max_vel_, max_acc_, max_jerk_;  // physical limits
    double local_traj_len_;                // local replanning trajectory length
    double ctrl_pt_dist;                   // distance between adjacient B-spline
//    // control points
//    double clearance_;
//    int dynamic_;
//    /* processing time */
//    double time_search_ = 0.0;
//    double time_optimize_ = 0.0;
//    double time_adjust_ = 0.0;
    double check_dist_;
    PlanParameters(double vel, double acc, double jerk) :
            max_vel_(vel), max_acc_(acc), max_jerk_(jerk) {};

};

struct BezierParameters {
    int traj_order;
    double minimize_order, cube_margin, obj;
    bool is_limit_vel, is_limit_acc;
    Eigen::MatrixXd MQM, FM, bezier_coeff;
    Eigen::VectorXd C, Cv, Ca, Cj;
};

struct MyPcd {
    double time;
    vector<float> x, y;

    MyPcd(double _t, vector<float> &_x, vector<float> &_y)
            : time(_t), x(_x), y(_y) {};

    typedef std::shared_ptr<MyPcd> Ptr;
};

class GlobalTrajData {
public:
    vector<Cube> corridor;
    vector<Eigen::Vector3d> fmm_path;
    Eigen::VectorXd path_time;

    int seg_num_;
    double global_duration_;
    double global_start_time_;

    BezierParameters bp_;
    Bernstein::Ptr bezier_base_;
    vector<NonUniformBspline> local_traj_;
    double local_start_time_, local_end_time_;
    double time_increase_;
    double last_time_inc_;
public:
    GlobalTrajData() : seg_num_(-1), global_duration_(-1), global_start_time_(-1),
    local_start_time_(-1), local_end_time_(-1), time_increase_(-1), last_time_inc_(-1){}

    ~GlobalTrajData() {
        vector<Cube>().swap(corridor);
        vector<Eigen::Vector3d>().swap(fmm_path);
    }

    void init(Bernstein::Ptr &_bezier) { this->bezier_base_ = _bezier; }

    void setBezierParameters(const BezierParameters &_bp) { this->bp_ = _bp; }

    // get Bspline paramterization data of a local trajectory within a sphere
    // start_t: start time of the trajectory
    // dist_pt: distance between the discretized points
    void getTrajByRadius(const double &start_t, const double &des_radius, const double &dist_pt,
                         vector<Eigen::Vector3d> &point_set, vector<Eigen::Vector3d> &start_end_derivative,
                         double &dt, double &seg_duration) {
        double seg_length = 0.0;  // length of the truncated segment
        double seg_time = 0.0;    // duration of the truncated segment
        double radius = 0.0;      // distance to the first point of the segment

        double delt = 0.2;
        Eigen::Vector3d first_pt = getPosition(start_t);  // first point of the segment
        Eigen::Vector3d prev_pt  = first_pt;               // previous point
        Eigen::Vector3d cur_pt;                           // current point

        while (radius < des_radius && start_t + seg_time < global_duration_ - 1e-3) {
            seg_time += delt;
            seg_time = min(seg_time, global_duration_);

            double t_global = start_t + seg_time;
            if(local_start_time_ >= 0 && t_global <= local_end_time_ - 1e-2){
                double t = t_global - local_start_time_;
                cur_pt = local_traj_[0].evaluateDeBoorT(t);
            } else
                cur_pt = getPosition(t_global);

            seg_length += (cur_pt - prev_pt).norm();
            prev_pt = cur_pt;
            radius  = (cur_pt - first_pt).norm();
        }

        // get parameterization dt by desired density of points
        int seg_num = floor(seg_length / dist_pt);

        // get outputs
        seg_duration = seg_time;  // duration of the truncated segment
        dt = seg_time / seg_num;  // time difference between to points

        point_set.clear();
        for (double tp = 0.0; tp <= seg_time + 1e-4; tp += dt) {
            double t_global = start_t + tp;
            if(local_start_time_ >= 0 && t_global <= local_end_time_ - 1e-3){
                double t = t_global - local_start_time_;
                cur_pt = local_traj_[0].evaluateDeBoorT(t);
            } else
                cur_pt = getPosition(t_global);

            point_set.push_back(cur_pt);
        }

        Eigen::Vector3d v0, v1, a0, a1;
        if(local_start_time_ >= 0 && start_t >= local_start_time_){
            v0 = local_traj_[1].evaluateDeBoorT(start_t - local_start_time_);
            a0 = local_traj_[2].evaluateDeBoorT(start_t - local_start_time_);
        } else {
            auto state0 = getState(start_t);
            v0 = state0.block<3, 1>(3, 0);
            a0 = state0.block<3, 1>(6, 0);
        }

        double tmp = start_t + seg_time;
        if(local_end_time_ >= 0 && tmp <= local_end_time_) {
            v1 = local_traj_[1].evaluateDeBoorT(tmp - local_start_time_);
            a1 = local_traj_[2].evaluateDeBoorT(tmp - local_start_time_);
        } else {
            auto state1 = getState(start_t + seg_time);
            v1 = state1.block<3, 1>(3, 0);
            a1 = state1.block<3, 1>(6, 0);
        }

        start_end_derivative.clear();
        start_end_derivative.push_back(v0);
        start_end_derivative.push_back(v1);
        start_end_derivative.push_back(a0);
        start_end_derivative.push_back(a1);
    }

    // get Bspline paramterization data of a fixed duration local trajectory
    // start_t: start time of the trajectory
    // duration: time length of the segment
    // seg_num: discretized the segment into *seg_num* parts
    void getTrajByDuration(double start_t, double duration, int seg_num,
                           vector<Eigen::Vector3d> &point_set,
                           vector<Eigen::Vector3d> &start_end_derivative, double &dt) {
        dt = duration / seg_num;

        Eigen::Vector3d cur_pt;
        point_set.clear();
        for (double tp = 0.0; tp <= duration + 1e-4; tp += dt) {
            if(local_start_time_ >= 0 && start_t + tp < local_end_time_ - 1e-3){
                cur_pt = local_traj_[0].evaluateDeBoorT(start_t + tp - local_start_time_);
            } else
                cur_pt = getPosition(start_t + tp);
            point_set.push_back(cur_pt);
        }

        Eigen::Vector3d v0, v1, a0, a1;
        if(local_start_time_ >= 0){
            v0 = local_traj_[1].evaluateDeBoorT(start_t - local_start_time_);
            a0 = local_traj_[2].evaluateDeBoorT(start_t - local_start_time_);
        }else{
            auto state0 = getState(start_t);
            v0 = state0.block<3, 1>(3, 0);
            a0 = state0.block<3, 1>(6, 0);
        }

        double tmp = start_t + duration;
        if(local_end_time_ >= 0 && tmp <= local_end_time_) {
            v1 = local_traj_[1].evaluateDeBoorT(tmp - local_start_time_);
            a1 = local_traj_[2].evaluateDeBoorT(tmp - local_start_time_);
        } else {
            auto state1 = getState(start_t + duration);
            v1 = state1.block<3, 1>(3, 0);
            a1 = state1.block<3, 1>(6, 0);
        }
        start_end_derivative.clear();

        start_end_derivative.push_back(v0);
        start_end_derivative.push_back(v1);
        start_end_derivative.push_back(a0);
        start_end_derivative.push_back(a1);
    }
    //获取bezier曲线的位置; t-全局轨迹的时间（起始处t=0s）
    Eigen::Vector3d getPosition(double t) {
        Eigen::Vector3d p;
        double t_s = max(0.0, t);
        int idx = 0;
        for (; idx < seg_num_; ++idx) {
            if (t_s > path_time(idx) && idx + 1 < seg_num_)
                t_s -= path_time(idx);
            else
                break;
        }

        p = bezier_base_->getPosFromBezier(bp_.bezier_coeff, t_s / path_time(idx), idx);
        for (int j = 0; j < 3; ++j) p(j) *= path_time(idx);

        return p;
    }
    //获取bezier曲线的状态; t-全局轨迹的时间（起始处t=0s）
    Eigen::VectorXd getState(double t) {
        Eigen::VectorXd p;

        double t_s = max(0.0, t);
        int idx = 0;
        for (; idx < seg_num_; ++idx) {
            if (t_s > path_time(idx) && idx + 1 < seg_num_)
                t_s -= path_time(idx);
            else
                break;
        }

        p = bezier_base_->getStateFromBezier(bp_.bezier_coeff, t_s / path_time(idx), idx);
        for (int j = 0; j < 12; ++j) {
            if (j < 3)
                p(j) *= path_time(idx);
            else if(j >= 6 && j < 9)
                p(j) /= path_time(idx);
            else if(j >= 9 && j < 12)
                p(j) /= pow(path_time(idx), 2);
        }

        return p;
    }

    void setLocalTraj(NonUniformBspline& traj, double local_ts, double local_te, double time_inc){
        local_traj_.resize(3);
        local_traj_[0] = traj;
        local_traj_[1] = local_traj_[0].getDerivative();
        local_traj_[2] = local_traj_[1].getDerivative();

        local_start_time_ = local_ts;
        local_end_time_   = local_te;
        global_duration_ += time_inc;
        time_increase_   += time_inc;
        last_time_inc_    = time_inc;
    }

    void setGlobalTraj(double start_time, const BezierParameters &_bp){
        global_duration_ = 0;
        path_time.resize(corridor.size());
        for(unsigned int i = 0; i < path_time.size(); i++){
            path_time[i] = corridor[i].t;
            global_duration_ += path_time[i];
        }

        bp_ = _bp;
        global_start_time_ = start_time;

        local_traj_.clear();
        local_start_time_ = -1;
        local_end_time_   = -1;
        time_increase_    = 0.0;
        last_time_inc_    = 0.0;
    }

    void setGlobalStartTime(ros::Time& t){
        global_start_time_ = t.toSec();
    }

    bool localTrajReachTarget() { return fabs(local_end_time_ - global_duration_) < 0.5; }
};

class MidPlanData {
public:
    MidPlanData() {}

    ~MidPlanData() {}

    //topological paths
    list<GraphNode::Ptr> topo_graph_;
    vector<vector<Eigen::Vector3d>> topo_paths_, topo_filtered_paths_, topo_select_paths_;

    void clearTopoPaths() {
        topo_traj_pos1_.clear();
        topo_traj_pos2_.clear();
        topo_graph_.clear();
        topo_paths_.clear();
        topo_filtered_paths_.clear();
        topo_select_paths_.clear();
    }

    // initial trajectory segment
    NonUniformBspline initial_local_segment_;
    vector<Eigen::Vector3d> local_start_end_derivative_;

    // multiple topological trajectories
    vector<NonUniformBspline> topo_traj_pos1_;
    vector<NonUniformBspline> topo_traj_pos2_;
    vector<NonUniformBspline> refines_;

    void addTopoPaths(list<GraphNode::Ptr> &graph, vector<vector<Eigen::Vector3d>> &paths,
                      vector<vector<Eigen::Vector3d>> &filtered_paths,
                      vector<vector<Eigen::Vector3d>> &selected_paths) {
        topo_graph_ = graph;
        topo_paths_ = paths;
        topo_filtered_paths_ = filtered_paths;
        topo_select_paths_ = selected_paths;
    }
};

struct LocalTrajData {
    /* info of generated traj */
    int    traj_id_;
    bool   is_traj_start_;
    double duration_;
    ros::Time start_time_;
    Eigen::Vector3d start_pos_;
    NonUniformBspline position_traj_, velocity_traj_, acceleration_traj_;
};

#endif //WMR_PLAN_MANAGE_PLAN_CONTAINER_HPP
